
#include "stm32f10x_conf.h"
#include "common.h"

#include "dynamixel_hal.h"

#define DX_RE_N_PIN         GPIO_Pin_11
#define DX_DE_PIN           GPIO_Pin_12

#define DX_RE_N_PORT        GPIOA
#define DX_DE_PORT          GPIOA

DynamixelHal::DynamixelHal(void)
{
  usart_busy = false;
  usart.init(this);

  GPIO_InitTypeDef GPIO_InitStructure;
  GPIO_INIT_MODE(DX_RE_N, GPIO_Mode_Out_PP);
  GPIO_INIT_MODE(DX_DE, GPIO_Mode_Out_PP);
  GPIO_RESET(DX_RE_N);
  GPIO_RESET(DX_DE);
}

void DynamixelHal::usartInterrupt(char data)
{
  instruction_receiver.appendChar(data);
}

void DynamixelHal::work(uint32_t time)
{
  // Hantera kö
  
  //usart.work();
}

bool DynamixelHal::writeByte(int id, int address, int value)
{
  if (instruction_packet.busy()) {
    return false;
  }
  instruction_packet = InstructionPacket(id, InstructionPacket::WRITE);
  instruction_packet.append(address);
  instruction_packet.append(value);
  instruction_packet.finalize();
  for (int idx = 0; idx < instruction_packet.buffer.size(); idx++) {
    usart.sendChar(instruction_packet.buffer[idx]);
  }
  GPIO_SET(DX_RE_N);
  GPIO_SET(DX_DE);
  // Pause before sending
  for (long i = 0; i < 500; i++);
  while (usart.work()) {}
  // Pause before receiving
  for (long i = 0; i < 1100; i++) {int j; j=1;}
  GPIO_RESET(DX_RE_N);
  GPIO_RESET(DX_DE);
  instruction_receiver.start();
  // Wait while receiving
  for (long i = 0; i < 20000; i++) {
    if (!instruction_receiver.isReceiving()) {
      return true;
    }
  }
  return false;
}

bool DynamixelHal::readByte(int id, int address, int* value)
{
  if (instruction_packet.busy()) {
    return false;
  }
  instruction_packet = InstructionPacket(id, InstructionPacket::READ);
  instruction_packet.append(address);
  instruction_packet.append(1);
  instruction_packet.finalize();
  for (int idx = 0; idx < instruction_packet.buffer.size(); idx++) {
    usart.sendChar(instruction_packet.buffer[idx]);
  }
  GPIO_SET(DX_RE_N);
  GPIO_SET(DX_DE);
  // Pause before sending
  for (long i = 0; i < 500; i++);
  while (usart.work()) {}
  // Pause before receiving
  for (long i = 0; i < 1100; i++) {int j; j=1;}
  GPIO_RESET(DX_RE_N);
  GPIO_RESET(DX_DE);
  instruction_receiver.start();
  // Wait while receiving
  for (long i = 0; i < 20000; i++) {
    if (!instruction_receiver.isReceiving()) {
      *value = instruction_receiver.buffer[5];
      return true;
    }
  }
  return false;
}

bool DynamixelHal::writeWord(int id, int address, int value)
{
  int value_high, value_low;
  value_low = value & 0xFF;
  value_high = (value >> 8) & 0xFF;
  if (instruction_packet.busy()) {
    return false;
  }
  instruction_packet = InstructionPacket(id, InstructionPacket::WRITE);
  instruction_packet.append(address);
  instruction_packet.append(value_low);
  instruction_packet.append(value_high);
  instruction_packet.finalize();
  for (int idx = 0; idx < instruction_packet.buffer.size(); idx++) {
    usart.sendChar(instruction_packet.buffer[idx]);
  }
  GPIO_SET(DX_RE_N);
  GPIO_SET(DX_DE);
  // Pause before sending
  for (long i = 0; i < 500; i++);
  while (usart.work()) {}
  // Pause before receiving
  for (long i = 0; i < 1100; i++) {int j; j=1;}
  GPIO_RESET(DX_RE_N);
  GPIO_RESET(DX_DE);
  instruction_receiver.start();
  // Wait while receiving
  for (long i = 0; i < 20000; i++) {
    if (!instruction_receiver.isReceiving()) {
      return true;
    }
  }
  return false;
}

bool DynamixelHal::readWord(int id, int address, int* value)
{
  int value_low, value_high;
  if (!readByte(id, address, &value_low)) {
    return false;
  }
  if (!readByte(id, address + 1, &value_high)) {
    return false;
  }
  *value = (value_high << 8) + value_low;
  return true;
}
